package br.ufrn.engcomp.observer;

import org.gnu.glpk.GLPK;
import org.gnu.glpk.GLPKConstants;
import org.gnu.glpk.SWIGTYPE_p_double;
import org.gnu.glpk.SWIGTYPE_p_int;
import org.gnu.glpk.glp_prob;
import org.gnu.glpk.glp_smcp;

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
/**
 *
 * @author allan
 */

//Observador com restrição de erro

public class ConstrainedObserver extends Observer {

    double eta = 0.1;
    SWIGTYPE_p_double val_p1;
    SWIGTYPE_p_double val_p2;
    SWIGTYPE_p_int ia_p1;
    SWIGTYPE_p_int ia_p2;
    SWIGTYPE_p_int ja_p1;
    SWIGTYPE_p_int ja_p2;
    //Matriz G = [Gf; C; -C]
    double[][] G = {
        {0.5453, -0.8383},
        {0.5212, -0.8535},
        {0.0000, 1.0000},
        {-0.5086, 0.8610},
        {-0.5212, 0.8535},
        {-0.5569, 0.8306},
        {-0.5453, 0.8383},
        {-0.5334, 0.8459},
        {-0.3977, -0.9175},
        {-0.0000, -1.0000},
        {0.5086, -0.8610},
        {0.5334, -0.8459},
        {0.3977, 0.9175},
        {0.5569, -0.8306},
        {0, 1}, {0, -1}
    };
    //Matriz rho_eta = [rhof; eta+y; eta-y]
    double[][] rho_eta = {
        {1.4680},
        {1.4018},
        {1.0000},
        {1.3696},
        {1.4018},
        {1.5021},
        {1.4680},
        {1.4346},
        {1.3152},
        {1.0000},
        {1.3696},
        {1.4346},
        {1.3152},
        {1.5021},
        {eta}, {eta}
    };
    private double[][] GiA = {
        {0.5187, -0.8221},
        {0.4948, -0.8370},
        {0.0191, 0.9807},
        {-0.4823, 0.8444},
        {-0.4948, 0.8370},
        {-0.5302, 0.8146},
        {-0.5187, 0.8221},
        {-0.5069, 0.8295},
        {-0.4076, -0.8998},
        {-0.0191, -0.9807},
        {0.4823, -0.8444},
        {0.5069, -0.8295},
        {0.4076, 0.8998},
        {0.5302, -0.8146}
    };
    //Matriz G_rho = [Gf -rhof]
    double[][] G_rho = new double[GiA.length][3];
    double[][] Psi = new double[GiA.length][1];

    public ConstrainedObserver() {

        val_p1 = GLPK.new_doubleArray(2 * G.length + 1);
        val_p2 = GLPK.new_doubleArray(3 * G_rho.length + 1);

        ia_p1 = GLPK.new_intArray(2 * G.length + 1);
        ia_p2 = GLPK.new_intArray(3 * G_rho.length + 1);

        ja_p1 = GLPK.new_intArray(2 * G.length + 1);
        ja_p2 = GLPK.new_intArray(3 * G_rho.length + 1);

        construirMatriz_G_Rho();
        construirTabelaDeRestricao_P1();
        construirTabelaDeRestricao_P2();
    }

    @Override
    public void calcularObservador(double saida) {
        rho_eta[rho_eta.length - 2][0] = eta + (saida - getYeq() - getDeltaX2());
        rho_eta[rho_eta.length - 1][0] = eta - (saida - getYeq() - getDeltaX2());
        LP1();
        //LP2();
        //valor de B calculado para o simulador
        //Bd = [0.149;0.000484]

        //valor de B calculado para os sitema de tanques 
        //Bd = [0.0675; 0.000212]

        //DeltaXc(k+1) = Ad*DeltaXc(k) + Bd*DeltaU(k) - V(z(k))

        setDeltaX((getAd()[0][0] * getDeltaX1() + getBd()[0][0] * getDeltaU() - getV1()),
                (getAd()[1][0] * getDeltaX1() + getAd()[1][1] * getDeltaX2() + getBd()[1][0] * getDeltaU() - getV2()));

        //Xc(k) = Xeq + DeltaXc(k)
        setXck(getXeq1() + getDeltaX1(), getXeq2() + getDeltaX2());

        // Erro = NivelTanqueReal - NivelEstimado
        //setErro(nivelTanque1 - getXck1(), saida - getXck2());
    }

    @Override
    public void run(double nivelTanque2) {
//        if (nivelTanque2 >= 14.8) {
            calcularObservador(nivelTanque2);
//        }
    }

    private void LP1() {
        for (int i = 0; i <= Psi.length - 1; i++) {
            resolve_P1(i);
        }
    }

    private void LP2() {
        glp_prob lp = GLPK.glp_create_prob();
        //definindo restrição
        //G_rho = [G -rho]
        GLPK.glp_add_rows(lp, G_rho.length);
//        System.out.println(" ------- Matriz Psi ------");
        for (int i = 0; i <= G_rho.length - 1; i++) {
            GLPK.glp_set_row_bnds(lp, i + 1, GLPKConstants.GLP_UP, 0, -1 * Psi[i][0]);
//            System.out.println(" ------------------------- " + Psi[i][0]);
        }

        //definindo objetivo
        // min -> epsi

        GLPK.glp_set_obj_dir(lp, GLPK.GLP_MIN);

        GLPK.glp_add_cols(lp, 3);

        GLPK.glp_set_col_name(lp, 1, "v1");
        GLPK.glp_set_col_kind(lp, 1, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 1, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 1, 0);
        GLPK.glp_set_col_name(lp, 2, "v2");
        GLPK.glp_set_col_kind(lp, 2, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 2, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 2, 0);
        GLPK.glp_set_col_name(lp, 3, "epsi");
        GLPK.glp_set_col_kind(lp, 3, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 3, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 3, 1);

        GLPK.glp_load_matrix(lp, 3 * G_rho.length, ia_p2, ja_p2, val_p2);
        //relovendo lp
        glp_smcp parm = new glp_smcp();
        GLPK.glp_init_smcp(parm);

        int resp = GLPK.glp_simplex(lp, parm);

        if (resp == 0) {
            setV1(GLPK.glp_get_col_prim(lp, 1));
            setV2(GLPK.glp_get_col_prim(lp, 2));
            System.out.println(GLPK.glp_get_col_name(lp, 1) + ": " + GLPK.glp_get_col_prim(lp, 1));
            System.out.println(GLPK.glp_get_col_name(lp, 2) + ": " + GLPK.glp_get_col_prim(lp, 2));
            System.out.println(GLPK.glp_get_col_name(lp, 3) + ": " + GLPK.glp_get_col_prim(lp, 3));
        } else {
//            System.out.println("-----------------------------------------The problem could not be solved");
        }
    }

    private void construirMatriz_G_Rho() {
        for (int i = 0; i <= G_rho.length - 1; i++) {
            G_rho[i][0] = G[i][0];
            G_rho[i][1] = G[i][1];
        }
        for (int i = 0; i <= G_rho.length - 1; i++) {
            G_rho[i][2] = -1 * rho_eta[i][0];
        }

//        for (int i = 0; i <= G_rho.length - 1; i++) {
//            for (int j = 0; j <= 2; j++) {
//                System.out.print(G_rho[i][j]);
//                System.out.print("   ");
//            }
//            System.out.println("");
//        }
    }

    private void construirTabelaDeRestricao_P1() {
        int n_linhas = G.length;
        int n_colunas = 2;
        //constroi vetor que representa os indices das linhas da matriz de restrições
        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ia_p1, 2 * (i - 1) + j, i);
//                System.out.println("Posição: " + (2 * (i - 1) + j) + " valor: " + GLPK.intArray_getitem(ia_p1, 2 * (i - 1) + j));
            }
        }
        //constroi vetor que reprsenta os indices das colunas da matriz de restiçoes
        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ja_p1, 2 * (i - 1) + j, j);
//                System.out.println("Posição: " + (2 * (i - 1) + j) + " valor: " + GLPK.intArray_getitem(ja_p1, 2*(i-1)+j));
            }
        }
        //constroi vetor com valores correspondente a cada indice da matriz de restriçoes
        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.doubleArray_setitem(val_p1, 2 * (i - 1) + j, G[i - 1][j - 1]);
//                System.out.println("Posição: " + (2 * (i - 1) + j) + " valor: " + GLPK.doubleArray_getitem(val_p1, 2 * (i - 1) + j));
            }
        }
    }

    private void construirTabelaDeRestricao_P2() {
        int n_linhas = G_rho.length;
        int n_colunas = 3;

        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ia_p2, 3 * (i - 1) + j, i);
//                System.out.println((3 * (i - 1) + j) + "----" + GLPK.intArray_getitem(ja_p2, 3 * (i - 1) + j));
            }
        }

        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ja_p2, 3 * (i - 1) + j, j);
//                System.out.println((3 * (i - 1) + j) + "----" + GLPK.intArray_getitem(ja_p2, 3 * (i - 1) + j));
            }
        }

        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.doubleArray_setitem(val_p2, 3 * (i - 1) + j, G_rho[i - 1][j - 1]);
//              System.out.println("Posição: " + (n_colunas*(i-1) + j) + " valor: " + GLPK.doubleArray_getitem(val_p2, (n_colunas*(i-1) + j)));
            }
        }
    }

    private void resolve_P1(int i) {
        glp_prob lp = GLPK.glp_create_prob();

        //definindo restrição
        //rho_eta = [rho;eta+y;eta-y]
        GLPK.glp_add_rows(lp, rho_eta.length);
        for (int j = 0; j <= rho_eta.length - 1; j++) {
            GLPK.glp_set_row_bnds(lp, j + 1, GLPKConstants.GLP_UP, 0, rho_eta[j][0]);
        }
        //definindo objetivo
        //objetivo -> max(Gi*Ad)
        GLPK.glp_set_obj_dir(lp, GLPKConstants.GLP_MAX);
        GLPK.glp_add_cols(lp, 2);
        GLPK.glp_set_col_name(lp, 1, "e1");
        GLPK.glp_set_col_kind(lp, 1, GLPKConstants.GLP_CV);// seta a coluna 1 como uma variavel continua 
        GLPK.glp_set_col_bnds(lp, 1, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 1, GiA[i][0]);
        GLPK.glp_set_col_name(lp, 1, "e2");
        GLPK.glp_set_col_kind(lp, 2, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 2, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 2, GiA[i][1]);

        GLPK.glp_load_matrix(lp, 2 * G.length, ia_p1, ja_p1, val_p1);

        //resolvendo modelo
        glp_smcp parm = new glp_smcp();
        GLPK.glp_init_smcp(parm);
        int resp = GLPK.glp_simplex(lp, parm);

        if (resp == 0) {
            Psi[i][0] = GLPK.glp_get_obj_val(lp);
            //System.out.println("Psi: " + Psi[p][0]);
        } else {
            System.out.println("The problem could not be solved");
        }
        //   deletando problema
        GLPK.glp_delete_prob(lp);
    }
}
